precisionAccelerometer.h File Reference

This graph shows which files directly or indirectly include this file:

Go to the source code of this file.

Data Structures
struct	AccelBuffer
struct	AccelCalibration

Macros
#define	ACCEL_CAL_AVERAGING_SECS   2
#define	MAX_ACCEL_CAL_ORIENTATIONS   12

Typedefs
typedef struct AccelBuffer	AccelBuffer
typedef struct AccelCalibration	AccelCalibration

Functions
void	fInitializeAccelCalibration (struct AccelCalibration *pthisAccelCal, struct AccelBuffer *pthisAccelBuffer, volatile int8_t *AccelCalPacketOn)
void	fUpdateAccelBuffer (struct AccelCalibration *pthisAccelCal, struct AccelBuffer *pthisAccelBuffer, struct AccelSensor *pthisAccel, volatile int8_t *AccelCalPacketOn)
void	fInvertAccelCal (struct AccelSensor *pthisAccel, struct AccelCalibration *pthisAccelCal)
void	fRunAccelCalibration (struct AccelCalibration *pthisAccelCal, struct AccelBuffer *pthisAccelBuffer, struct AccelSensor *pthisAccel)
void	fComputeAccelCalibration4 (struct AccelBuffer *pthisAccelBuffer, struct AccelCalibration *pthisAccelCal, struct AccelSensor *pthisAccel)
void	fComputeAccelCalibration7 (struct AccelBuffer *pthisAccelBuffer, struct AccelCalibration *pthisAccelCal, struct AccelSensor *pthisAccel)
void	fComputeAccelCalibration10 (struct AccelBuffer *pthisAccelBuffer, struct AccelCalibration *pthisAccelCal, struct AccelSensor *pthisAccel)

Macro Definition Documentation

#define ACCEL_CAL_AVERAGING_SECS   2

calibration constants

calibration measurement averaging period (s)

Definition at line 34 of file precisionAccelerometer.h.

Referenced by DecodeCommandBytes(), and fUpdateAccelBuffer().

#define MAX_ACCEL_CAL_ORIENTATIONS   12

number of stored precision accelerometer measurements

Definition at line 35 of file precisionAccelerometer.h.

Referenced by CreateAndSendPackets(), fComputeAccelCalibration10(), fComputeAccelCalibration4(), fComputeAccelCalibration7(), fInitializeAccelCalibration(), and fRunAccelCalibration().

Typedef Documentation

typedef struct AccelBuffer AccelBuffer

accelerometer measurement buffer

typedef struct AccelCalibration AccelCalibration

precision accelerometer calibration structure

Function Documentation

void fComputeAccelCalibration10	(	struct AccelBuffer *	pthisAccelBuffer,
		struct AccelCalibration *	pthisAccelCal,
		struct AccelSensor *	pthisAccel
	)

calculate the 10 element calibration from the available measurements

Parameters

pthisAccelBuffer	Buffer of measurements used as input to the accel calibration functions
pthisAccelCal	Accelerometer calibration parameter structure
pthisAccel	Pointer to the accelerometer input/state structure

Definition at line 442 of file precisionAccelerometer.c.

Referenced by fRunAccelCalibration().

{
    int32   i,
            j,
            k,
            l,
            m,
            n;                  // loop counters
    float   det;                // matrix determinant
    float   ftmp;               // scratch
    float   fg0;                // fitted local gravity magnitude

    // zero the on and above diagonal elements of the 10x10 symmetric measurement matrix fmatA
    for (i = 0; i < 10; i++)
        for (j = i; j < 10; j++) pthisAccelCal->fmatA[i][j] = 0.0F;

    // last entry of vector fvecA is always 1.0 so move assignment outside the loop
    pthisAccelCal->fvecA[9] = 1.0F;

    // loop over all orientations
    for (i = 0; i < MAX_ACCEL_CAL_ORIENTATIONS; i++)
    {
        // accumulate fvecA if this entry is valid
        if ((pthisAccelBuffer->iStoreFlags) & (1 << i))
        {
            // compute the remaining members of the measurement vector fvecA
            pthisAccelCal->fvecA[6] = pthisAccelBuffer->fGsStored[i][CHX];
            pthisAccelCal->fvecA[7] = pthisAccelBuffer->fGsStored[i][CHY];
            pthisAccelCal->fvecA[8] = pthisAccelBuffer->fGsStored[i][CHZ];
            pthisAccelCal->fvecA[0] = pthisAccelCal->fvecA[6] * pthisAccelCal->fvecA[6];
            pthisAccelCal->fvecA[1] = 2.0F *
                pthisAccelCal->fvecA[6] *
                pthisAccelCal->fvecA[7];
            pthisAccelCal->fvecA[2] = 2.0F *
                pthisAccelCal->fvecA[6] *
                pthisAccelCal->fvecA[8];
            pthisAccelCal->fvecA[3] = pthisAccelCal->fvecA[7] * pthisAccelCal->fvecA[7];
            pthisAccelCal->fvecA[4] = 2.0F *
                pthisAccelCal->fvecA[7] *
                pthisAccelCal->fvecA[8];
            pthisAccelCal->fvecA[5] = pthisAccelCal->fvecA[8] * pthisAccelCal->fvecA[8];

            // accumulate the on-and above-diagonal terms of fmatA=Sigma{fvecA^T * fvecA}
            for (m = 0; m < 10; m++)
            {
                for (n = m; n < 10; n++)
                {
                    pthisAccelCal->fmatA[m][n] += pthisAccelCal->fvecA[m] * pthisAccelCal->fvecA[n];
                }
            }
        }                       // end of test for stored data
    }                           // end of loop over orientations

    // copy the above diagonal elements of symmetric product matrix fmatA to below the diagonal
    for (m = 1; m < 10; m++)
        for (n = 0; n < m; n++)
            pthisAccelCal->fmatA[m][n] = pthisAccelCal->fmatA[n][m];

    // set fvecA to the unsorted eigenvalues and fmatB to the unsorted normalized eigenvectors of fmatA
    fEigenCompute10(pthisAccelCal->fmatA, pthisAccelCal->fvecA,
                    pthisAccelCal->fmatB, 10);

    // set ellipsoid matrix A from elements of the solution vector column j with smallest eigenvalue
    j = 0;
    for (i = 1; i < 10; i++)
    {
        if (pthisAccelCal->fvecA[i] < pthisAccelCal->fvecA[j])
        {
            j = i;
        }
    }

    pthisAccelCal->fA[0][0] = pthisAccelCal->fmatB[0][j];
    pthisAccelCal->fA[0][1] = pthisAccelCal->fA[1][0] = pthisAccelCal->fmatB[1][j];
    pthisAccelCal->fA[0][2] = pthisAccelCal->fA[2][0] = pthisAccelCal->fmatB[2][j];
    pthisAccelCal->fA[1][1] = pthisAccelCal->fmatB[3][j];
    pthisAccelCal->fA[1][2] = pthisAccelCal->fA[2][1] = pthisAccelCal->fmatB[4][j];
    pthisAccelCal->fA[2][2] = pthisAccelCal->fmatB[5][j];

    // negate entire solution if A has negative determinant
    det = f3x3matrixDetA(pthisAccelCal->fA);
    if (det < 0.0F)
    {
        f3x3matrixAeqMinusA(pthisAccelCal->fA);
        pthisAccelCal->fmatB[6][j] = -pthisAccelCal->fmatB[6][j];
        pthisAccelCal->fmatB[7][j] = -pthisAccelCal->fmatB[7][j];
        pthisAccelCal->fmatB[8][j] = -pthisAccelCal->fmatB[8][j];
        pthisAccelCal->fmatB[9][j] = -pthisAccelCal->fmatB[9][j];
        det = -det;
    }

    // compute the inverse of the ellipsoid matrix
    f3x3matrixAeqInvSymB(pthisAccelCal->finvA, pthisAccelCal->fA);

    // compute the offset vector V
    for (l = CHX; l <= CHZ; l++)
    {
        pthisAccelCal->fV[l] = 0.0F;
        for (m = CHX; m <= CHZ; m++)
        {
            pthisAccelCal->fV[l] += pthisAccelCal->finvA[l][m] * pthisAccelCal->fmatB[m + 6][j];
        }

        pthisAccelCal->fV[l] *= -0.5F;
    }

    // compute the local gravity fit to these calibration coefficients
    fg0 = sqrtf(fabsf(pthisAccelCal->fA[0][0] * pthisAccelCal->fV[CHX] *
                pthisAccelCal->fV[CHX] + 2.0F * pthisAccelCal->fA[0][1] *
                pthisAccelCal->fV[CHX] * pthisAccelCal->fV[CHY] + 2.0F *
                pthisAccelCal->fA[0][2] * pthisAccelCal->fV[CHX] *
                pthisAccelCal->fV[CHZ] + pthisAccelCal->fA[1][1] *
                pthisAccelCal->fV[CHY] * pthisAccelCal->fV[CHY] + 2.0F *
                pthisAccelCal->fA[1][2] * pthisAccelCal->fV[CHY] *
                pthisAccelCal->fV[CHZ] + pthisAccelCal->fA[2][2] *
                pthisAccelCal->fV[CHZ] * pthisAccelCal->fV[CHZ] -
                pthisAccelCal->fmatB[9][j]));

    // compute trial invW from the square root of fA    
    // set fvecA to the unsorted eigenvalues and fmatB to the unsorted eigenvectors of fmatA
    // where fmatA holds the 3x3 matrix fA in its top left elements
    for (i = 0; i < 3; i++)
        for (j = 0; j < 3; j++)
            pthisAccelCal->fmatA[i][j] = pthisAccelCal->fA[i][j];
    fEigenCompute10(pthisAccelCal->fmatA, pthisAccelCal->fvecA,
                    pthisAccelCal->fmatB, 3);

    // set fmatB to be eigenvectors . diag(sqrt(sqrt(eigenvalues))) = fmatB . diag(sqrt(sqrt(fvecA))
    for (j = 0; j < 3; j++)     // loop over columns j
    {
        ftmp = sqrtf(sqrtf(fabsf(pthisAccelCal->fvecA[j])));
        for (i = 0; i < 3; i++) // loop over rows i
        {
            pthisAccelCal->fmatB[i][j] *= ftmp;
        }
    }

    // set finvW to eigenvectors * diag(sqrt(eigenvalues)) * eigenvectors^T
    // = fmatB * fmatB^T = sqrt(fA) (guaranteed symmetric)
    // loop over rows
    for (i = 0; i < 3; i++)
    {
        // loop over on and above diagonal columns
        for (j = i; j < 3; j++)
        {
            pthisAccelCal->finvW[i][j] = 0.0F;

            // accumulate the matrix product
            for (k = 0; k < 3; k++)
            {
                pthisAccelCal->finvW[i][j] += pthisAccelCal->fmatB[i][k] * pthisAccelCal->fmatB[j][k];
            }

            // copy to below diagonal element
            pthisAccelCal->finvW[j][i] = pthisAccelCal->finvW[i][j];
        }
    }

    // scale finvW so that the calibrated measurements fit on the 1g sphere
    for (i = CHX; i <= CHZ; i++)
    {
        for (j = CHX; j <= CHZ; j++)
        {
            pthisAccelCal->finvW[i][j] /= fg0;
        }
    }

    return;
}

Here is the call graph for this function:

Here is the caller graph for this function:

void fComputeAccelCalibration4	(	struct AccelBuffer *	pthisAccelBuffer,
		struct AccelCalibration *	pthisAccelCal,
		struct AccelSensor *	pthisAccel
	)

calculate the 4 element calibration from the available measurements

Parameters

pthisAccelBuffer	Buffer of measurements used as input to the accel calibration functions
pthisAccelCal	Accelerometer calibration parameter structure
pthisAccel	Pointer to the accelerometer input/state structure

Definition at line 233 of file precisionAccelerometer.c.

Referenced by fRunAccelCalibration().

{
    int32   i,
            j;      // loop counters
    float   ftmp;   // scratch
    int8_t  ierror; // flag from matrix inversion

    // working arrays for 4x4 matrix inversion
    float   *pfRows[4];
    int8_t  iColInd[4];
    int8_t  iRowInd[4];
    int8_t  iPivot[4];

    // zero the 4x4 matrix XTX (in upper left of fmatA) and 4x1 vector XTY (in upper fvecA)
    for (i = 0; i < 4; i++)
    {
        pthisAccelCal->fvecA[i] = 0.0F;
        for (j = 0; j < 4; j++)
        {
            pthisAccelCal->fmatA[i][j] = 0.0F;
        }
    }

    // accumulate fXTY (in fvecA) and fXTX4x4 (in fmatA)
    for (i = 0; i < MAX_ACCEL_CAL_ORIENTATIONS; i++)
    {
        // accumulate vector X^T.Y if this entry is valid
        if ((pthisAccelBuffer->iStoreFlags) & (1 << i))
        {
            ftmp = pthisAccelBuffer->fGsStored[i][CHX] *
                pthisAccelBuffer->fGsStored[i][CHX] +
                pthisAccelBuffer->fGsStored[i][CHY] *
                pthisAccelBuffer->fGsStored[i][CHY] +
                pthisAccelBuffer->fGsStored[i][CHZ] *
                pthisAccelBuffer->fGsStored[i][CHZ];
            pthisAccelCal->fvecA[0] += pthisAccelBuffer->fGsStored[i][CHX] * ftmp;
            pthisAccelCal->fvecA[1] += pthisAccelBuffer->fGsStored[i][CHY] * ftmp;
            pthisAccelCal->fvecA[2] += pthisAccelBuffer->fGsStored[i][CHZ] * ftmp;
            pthisAccelCal->fvecA[3] += ftmp;

            // accumulate above diagonal terms of matrix X^T.X
            pthisAccelCal->fmatA[CHX][CHX] += pthisAccelBuffer->fGsStored[i][
                    CHX] *
                pthisAccelBuffer->fGsStored[i][CHX];
            pthisAccelCal->fmatA[CHX][CHY] += pthisAccelBuffer->fGsStored[i][
                    CHX] *
                pthisAccelBuffer->fGsStored[i][CHY];
            pthisAccelCal->fmatA[CHX][CHZ] += pthisAccelBuffer->fGsStored[i][
                    CHX] *
                pthisAccelBuffer->fGsStored[i][CHZ];
            pthisAccelCal->fmatA[CHX][3] += pthisAccelBuffer->fGsStored[i][CHX];
            pthisAccelCal->fmatA[CHY][CHY] += pthisAccelBuffer->fGsStored[i][
                    CHY] *
                pthisAccelBuffer->fGsStored[i][CHY];
            pthisAccelCal->fmatA[CHY][CHZ] += pthisAccelBuffer->fGsStored[i][
                    CHY] *
                pthisAccelBuffer->fGsStored[i][CHZ];;
            pthisAccelCal->fmatA[CHY][3] += pthisAccelBuffer->fGsStored[i][CHY];
            pthisAccelCal->fmatA[CHZ][CHZ] += pthisAccelBuffer->fGsStored[i][
                    CHZ] *
                pthisAccelBuffer->fGsStored[i][CHZ];;
            pthisAccelCal->fmatA[CHZ][3] += pthisAccelBuffer->fGsStored[i][CHZ];
            pthisAccelCal->fmatA[3][3] += 1.0F;
        }
    }

    // copy above diagonal elements of fXTX4x4 = X^T.X to below diagonal elements
    pthisAccelCal->fmatA[CHY][CHX] = pthisAccelCal->fmatA[CHX][CHY];
    pthisAccelCal->fmatA[CHZ][CHX] = pthisAccelCal->fmatA[CHX][CHZ];
    pthisAccelCal->fmatA[CHZ][CHY] = pthisAccelCal->fmatA[CHY][CHZ];
    pthisAccelCal->fmatA[3][CHX] = pthisAccelCal->fmatA[CHX][3];
    pthisAccelCal->fmatA[3][CHY] = pthisAccelCal->fmatA[CHY][3];
    pthisAccelCal->fmatA[3][CHZ] = pthisAccelCal->fmatA[CHZ][3];

    // calculate in situ inverse of X^T.X
    for (i = 0; i < 4; i++)
    {
        pfRows[i] = pthisAccelCal->fmatA[i];
    }

    fmatrixAeqInvA(pfRows, iColInd, iRowInd, iPivot, 4, &ierror);

    // calculate the solution vector fvecB = inv(X^T.X).X^T.Y
    for (i = 0; i < 4; i++)
    {
        pthisAccelCal->fvecB[i] = 0.0F;
        for (j = 0; j < 4; j++)
        {
            pthisAccelCal->fvecB[i] += pthisAccelCal->fmatA[i][j] * pthisAccelCal->fvecA[j];
        }
    }

    // extract the offset vector
    pthisAccelCal->fV[CHX] = 0.5F * pthisAccelCal->fvecB[CHX];
    pthisAccelCal->fV[CHY] = 0.5F * pthisAccelCal->fvecB[CHY];
    pthisAccelCal->fV[CHZ] = 0.5F * pthisAccelCal->fvecB[CHZ];

    // set ftmp to 1/W where W is the forward gain to fit the 1g sphere
    ftmp = 1.0F / sqrtf(fabsf(pthisAccelCal->fvecB[3] + pthisAccelCal->fV[CHX] *
                        pthisAccelCal->fV[CHX] + pthisAccelCal->fV[CHY] *
                        pthisAccelCal->fV[CHY] + pthisAccelCal->fV[CHZ] *
                        pthisAccelCal->fV[CHZ]));

    // copy the inverse gain 1/W to the inverse gain matrix
    pthisAccelCal->finvW[CHX][CHY] = pthisAccelCal->finvW[CHY][CHX] = 0.0F;
    pthisAccelCal->finvW[CHX][CHZ] = pthisAccelCal->finvW[CHZ][CHX] = 0.0F;
    pthisAccelCal->finvW[CHY][CHZ] = pthisAccelCal->finvW[CHZ][CHY] = 0.0F;
    pthisAccelCal->finvW[CHX][CHX] = pthisAccelCal->finvW[CHY][CHY] = pthisAccelCal->finvW[CHZ][CHZ] = ftmp;

    return;
}

Here is the call graph for this function:

Here is the caller graph for this function:

void fComputeAccelCalibration7	(	struct AccelBuffer *	pthisAccelBuffer,
		struct AccelCalibration *	pthisAccelCal,
		struct AccelSensor *	pthisAccel
	)

calculate the 7 element calibration from the available measurements

Parameters

pthisAccelBuffer	Buffer of measurements used as input to the accel calibration functions
pthisAccelCal	Accelerometer calibration parameter structure
pthisAccel	Pointer to the accelerometer input/state structure

Definition at line 348 of file precisionAccelerometer.c.

Referenced by fRunAccelCalibration().

{
    int32   i,
            j,
            m,
            n;      // loop counters
    float   det;    // matrix determinant
    float   fg0;    // fitted local gravity magnitude

    // zero the on and above diagonal elements of the 7x7 symmetric measurement matrix fmatA
    for (i = 0; i < 7; i++)
        for (j = i; j < 7; j++) pthisAccelCal->fmatA[i][j] = 0.0F;

    // last entry of vector fvecA is always 1.0 so move assignment outside the loop
    pthisAccelCal->fvecA[6] = 1.0F;

    // loop over all orientations
    for (i = 0; i < MAX_ACCEL_CAL_ORIENTATIONS; i++)
    {
        // accumulate fvecA if this entry is valid
        if ((pthisAccelBuffer->iStoreFlags) & (1 << i))
        {
            // compute the remaining members of the measurement vector fvecA
            pthisAccelCal->fvecA[0] = pthisAccelBuffer->fGsStored[i][CHX] * pthisAccelBuffer->fGsStored[i][CHX];
            pthisAccelCal->fvecA[1] = pthisAccelBuffer->fGsStored[i][CHY] * pthisAccelBuffer->fGsStored[i][CHY];
            pthisAccelCal->fvecA[2] = pthisAccelBuffer->fGsStored[i][CHZ] * pthisAccelBuffer->fGsStored[i][CHZ];
            pthisAccelCal->fvecA[3] = pthisAccelBuffer->fGsStored[i][CHX];
            pthisAccelCal->fvecA[4] = pthisAccelBuffer->fGsStored[i][CHY];
            pthisAccelCal->fvecA[5] = pthisAccelBuffer->fGsStored[i][CHZ];

            // accumulate the on-and above-diagonal terms of fmatA=Sigma{fvecA^T * fvecA}
            for (m = 0; m < 7; m++)
                for (n = m; n < 7; n++)
                    pthisAccelCal->fmatA[m][n] += pthisAccelCal->fvecA[m] * pthisAccelCal->fvecA[n];
        }           // end of test for stored data
    }               // end of loop over orientations

    // copy the above diagonal elements of symmetric product matrix fmatA to below the diagonal
    for (m = 1; m < 7; m++)
        for (n = 0; n < m; n++)
            pthisAccelCal->fmatA[m][n] = pthisAccelCal->fmatA[n][m];

    // set fvecA to the unsorted eigenvalues and fmatB to the unsorted normalized eigenvectors of fmatA
    fEigenCompute10(pthisAccelCal->fmatA, pthisAccelCal->fvecA,
                    pthisAccelCal->fmatB, 7);

    // set ellipsoid matrix A from elements of the solution vector column j with smallest eigenvalue
    j = 0;
    for (i = 1; i < 7; i++)
        if (pthisAccelCal->fvecA[i] < pthisAccelCal->fvecA[j]) j = i;

    // negate the entire solution vector if ellipsoid matrix has negative determinant
    det = pthisAccelCal->fmatB[0][j] *
        pthisAccelCal->fmatB[1][j] *
        pthisAccelCal->fmatB[2][j];
    if (det < 0.0F)
    {
        for (i = 0; i < 7; i++)
        {
            pthisAccelCal->fmatB[i][j] = -pthisAccelCal->fmatB[i][j];
        }
    }

    // compute invW and V and fitted gravity g0 from solution vector j
    f3x3matrixAeqScalar(pthisAccelCal->finvW, 0.0F);
    pthisAccelCal->finvW[CHX][CHX] = sqrtf(fabsf(pthisAccelCal->fmatB[0][j]));
    pthisAccelCal->finvW[CHY][CHY] = sqrtf(fabsf(pthisAccelCal->fmatB[1][j]));
    pthisAccelCal->finvW[CHZ][CHZ] = sqrtf(fabsf(pthisAccelCal->fmatB[2][j]));
    pthisAccelCal->fV[CHX] = -0.5F *
        pthisAccelCal->fmatB[3][j] /
        pthisAccelCal->fmatB[0][j];
    pthisAccelCal->fV[CHY] = -0.5F *
        pthisAccelCal->fmatB[4][j] /
        pthisAccelCal->fmatB[1][j];
    pthisAccelCal->fV[CHZ] = -0.5F *
        pthisAccelCal->fmatB[5][j] /
        pthisAccelCal->fmatB[2][j];
    fg0 = sqrtf(fabsf(pthisAccelCal->fmatB[0][j] * pthisAccelCal->fV[CHX] *
                pthisAccelCal->fV[CHX] + pthisAccelCal->fmatB[1][j] *
                pthisAccelCal->fV[CHY] * pthisAccelCal->fV[CHY] +
                pthisAccelCal->fmatB[2][j] * pthisAccelCal->fV[CHZ] *
                pthisAccelCal->fV[CHZ] - pthisAccelCal->fmatB[6][j]));

    // normalize invW to fit the 1g sphere
    pthisAccelCal->finvW[CHX][CHX] /= fg0;
    pthisAccelCal->finvW[CHY][CHY] /= fg0;
    pthisAccelCal->finvW[CHZ][CHZ] /= fg0;

    return;
}

Here is the call graph for this function:

Here is the caller graph for this function:

void fInitializeAccelCalibration	(	struct AccelCalibration *	pthisAccelCal,
		struct AccelBuffer *	pthisAccelBuffer,
		volatile int8_t *	AccelCalPacketOn
	)

Initialize the accelerometer calibration functions.

Parameters

pthisAccelCal	Accelerometer calibration parameter structure
pthisAccelBuffer	Buffer of measurements used as input to the accel calibration functions
AccelCalPacketOn	Used to coordinate calibration sample storage and communications

Definition at line 34 of file precisionAccelerometer.c.

Referenced by DecodeCommandBytes(), and initializeFusionEngine().

{
    float   *pFlash;    // pointer into flash memory
    int8_t  i,
            j;          // loop counters

    // set flags to false to denote no precision accelerometer measurements
    pthisAccelBuffer->iStoreFlags = 0;

    // perform one transmission of the precision calibration (using invalid value MAX_ACCEL_CAL_ORIENTATIONS for calibration only)
    *AccelCalPacketOn = MAX_ACCEL_CAL_ORIENTATIONS;

    // check to see if the stored accelerometer calibration has been erased
    // the standard value for erased flash is 0xFF in each byte but for portability check against 0x12345678
    pFlash = (float *) (CALIBRATION_NVM_ADDR + ACCEL_NVM_OFFSET);
    if (*((uint32 *) pFlash++) == 0x12345678)
    {
        // a precision accelerometer calibration is present in flash
        // copy accelerometer calibration elements (21x float total 84 bytes) from flash to RAM
        for (i = CHX; i <= CHZ; i++) pthisAccelCal->fV[i] = *(pFlash++);
        for (i = CHX; i <= CHZ; i++)
            for (j = CHX; j <= CHZ; j++)
                pthisAccelCal->finvW[i][j] = *(pFlash++);
        for (i = CHX; i <= CHZ; i++)
            for (j = CHX; j <= CHZ; j++)
                pthisAccelCal->fR0[i][j] = *(pFlash++);
    }
    else
    {
        // flash has been erased and no accelerometer calibration is present
        // initialize the precision accelerometer calibration in RAM to null default
        pthisAccelCal->fV[CHX] = pthisAccelCal->fV[CHY] = pthisAccelCal->fV[CHZ] = 0.0F;
        f3x3matrixAeqI(pthisAccelCal->finvW);
        f3x3matrixAeqI(pthisAccelCal->fR0);
    }

    // set current averaging location and counter to -1 for invalid
    pthisAccelBuffer->iStoreLocation = pthisAccelBuffer->iStoreCounter = -1;

    return;
}

Here is the call graph for this function:

Here is the caller graph for this function:

void fInvertAccelCal	(	struct AccelSensor *	pthisAccel,
		struct AccelCalibration *	pthisAccelCal
	)

function maps the accelerometer data fGs (g) onto precision calibrated and de-rotated data fGc (g), iGc (counts)

Parameters

pthisAccel	Pointer to the accelerometer input/state structure
pthisAccelCal	Accelerometer calibration parameter structure

Definition at line 130 of file precisionAccelerometer.c.

Referenced by initializeSensors().

{
    // local variables
    float   ftmp[3];    // temporary array
    int8_t  i;          // loop counter

    //subtract the offset vector fV (g): ftmp[]=fGs[]-V[]
    for (i = CHX; i <= CHZ; i++)
        ftmp[i] = pthisAccel->fGs[i] - pthisAccelCal->fV[i];

    // apply the inverse rotation correction matrix finvW: fGc=inv(W)*(fGs[]-V[])
    for (i = CHX; i <= CHZ; i++)
    {
        pthisAccel->fGc[i] = pthisAccelCal->finvW[i][CHX] *
            ftmp[CHX] +
            pthisAccelCal->finvW[i][CHY] *
            ftmp[CHY] +
            pthisAccelCal->finvW[i][CHZ] *
            ftmp[CHZ];
    }

    // apply the inverse of the forward rotation matrix fR0: fGc=inv(R).inv(W)*(fGs[]-V[])
    for (i = CHX; i <= CHZ; i++) ftmp[i] = pthisAccel->fGc[i];
    for (i = CHX; i <= CHZ; i++)
    {
        pthisAccel->fGc[i] = pthisAccelCal->fR0[CHX][i] *
            ftmp[CHX] +
            pthisAccelCal->fR0[CHY][i] *
            ftmp[CHY] +
            pthisAccelCal->fR0[CHZ][i] *
            ftmp[CHZ];
        pthisAccel->iGc[i] = (int16_t) (pthisAccel->fGc[i] * pthisAccel->iCountsPerg);
    }

    return;
}

Here is the caller graph for this function:

void fRunAccelCalibration	(	struct AccelCalibration *	pthisAccelCal,
		struct AccelBuffer *	pthisAccelBuffer,
		struct AccelSensor *	pthisAccel
	)

function runs the precision accelerometer calibration

Parameters

pthisAccelCal	Accelerometer calibration parameter structure
pthisAccelBuffer	Buffer of measurements used as input to the accel calibration functions
pthisAccel	Pointer to the accelerometer input/state structure

Definition at line 169 of file precisionAccelerometer.c.

Referenced by fUpdateAccelBuffer().

{
    float   fGc0[3];        // calibrated but not de-rotated measurement 0
    uint8_t iMeasurements;  // number of stored measurements
    int8_t  i;              // loop counters

    // calculate how many measurements are present in the accelerometer measurement buffer
    iMeasurements = 0;
    for (i = 0; i < MAX_ACCEL_CAL_ORIENTATIONS; i++)
    {
        if (pthisAccelBuffer->iStoreFlags & (1 << i)) iMeasurements++;
    }

    // perform the highest quality calibration possible given this number
    if (iMeasurements >= 9)
    {
        // perform the 10 element calibration
        fComputeAccelCalibration10(pthisAccelBuffer, pthisAccelCal, pthisAccel);
    }
    else if (iMeasurements >= 6)
    {
        // perform the 7 element calibration
        fComputeAccelCalibration7(pthisAccelBuffer, pthisAccelCal, pthisAccel);
    }
    else if (iMeasurements >= 4)
    {
        // perform the 4 element calibration
        fComputeAccelCalibration4(pthisAccelBuffer, pthisAccelCal, pthisAccel);
    }

    // calculate the rotation correction matrix to rotate calibrated measurement 0 to flat
    if (pthisAccelBuffer->iStoreFlags & 1)
    {
        // apply offset and gain calibration but not rotation to measurement 0 (flat) if present
        // set ftmpA3x1 = invW . (fGs - fV)
        for (i = CHX; i <= CHZ; i++)
            fGc0[i] = pthisAccelBuffer->fGsStored[0][i] - pthisAccelCal->fV[i];
        fVeq3x3AxV(fGc0, pthisAccelCal->finvW);

        // compute the new final rotation matrix if rotation 0 (flat) is present.
        // multiplying by the transpose of this matrix therefore forces measurement zero to flat.
        switch (THISCOORDSYSTEM)
        {
            case NED:
                f3DOFTiltNED(pthisAccelCal->fR0, fGc0);
                break;

            case ANDROID:
                f3DOFTiltAndroid(pthisAccelCal->fR0, fGc0);
                break;

            case WIN8:
            default:
                f3DOFTiltWin8(pthisAccelCal->fR0, fGc0);
                break;
        }
    }

    return;
}

Here is the call graph for this function:

Here is the caller graph for this function:

void fUpdateAccelBuffer	(	struct AccelCalibration *	pthisAccelCal,
		struct AccelBuffer *	pthisAccelBuffer,
		struct AccelSensor *	pthisAccel,
		volatile int8_t *	AccelCalPacketOn
	)

Update the buffer used to store samples used for accelerometer calibration.

Parameters

pthisAccelCal	Accelerometer calibration parameter structure
pthisAccelBuffer	Buffer of measurements used as input to the accel calibration functions
pthisAccel	Pointer to the accelerometer input/state structure
AccelCalPacketOn	Used to coordinate calibration sample storage and communications

Definition at line 78 of file precisionAccelerometer.c.

Referenced by initializeSensors().

{
    int16_t i;          // loop counter

    // iStoreCounter > 0: precision measurements are still on-going
    // iStoreCounter = 0: the precision measurement has just finished
    // iStoreCounter = -1: no precision measurement is in progress
    // check if a new precision measurement has started and zero sums if one has
    if (pthisAccelBuffer->iStoreCounter == (ACCEL_CAL_AVERAGING_SECS * FUSION_HZ))
    {
        for (i = CHX; i <= CHZ; i++) pthisAccelBuffer->fSumGs[i] = 0.0F;
    }

    // accumulate sum if averaging not yet complete
    if (pthisAccelBuffer->iStoreCounter > 0)
    {
        for (i = CHX; i <= CHZ; i++)
            pthisAccelBuffer->fSumGs[i] += pthisAccel->fGs[i];
        pthisAccelBuffer->iStoreCounter--;
    }

    // check if the measurement accumulation is complete and, if so, store average and set packet transmit flag
    if (pthisAccelBuffer->iStoreCounter == 0)
    {
        // store the measurement, set the relevant flag and decrement the counter down to -1 denoting completion
        for (i = CHX; i <= CHZ; i++)
        {
            pthisAccelBuffer->fGsStored[pthisAccelBuffer->iStoreLocation][i] =
                    pthisAccelBuffer->fSumGs[i] /
                (float) (ACCEL_CAL_AVERAGING_SECS * FUSION_HZ);
        }

        pthisAccelBuffer->iStoreFlags |=
            (
                1 <<
                pthisAccelBuffer->iStoreLocation
            );
        pthisAccelBuffer->iStoreCounter--;

        // compute the new precision accelerometer calibration including rotation using all measurements
        fRunAccelCalibration(pthisAccelCal, pthisAccelBuffer, pthisAccel);

        // and make one packet transmission of this measurement with the new calibration
        *AccelCalPacketOn = pthisAccelBuffer->iStoreLocation;
    }

    return;
}

Here is the call graph for this function:

Here is the caller graph for this function: